{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "name": "Robotic Manipulation - Introduction.ipynb",
      "provenance": [],
      "collapsed_sections": [],
      "toc_visible": true
    },
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.6.9-final"
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "TKvYiJgnYExi"
      },
      "source": [
        "Welcome!  If you are new to Google Colab/Jupyter notebooks, you might take a look at [this notebook](https://colab.research.google.com/notebooks/basic_features_overview.ipynb) first.\n",
        "\n",
        "**I recommend you run the first code cell of this notebook immediately, to start provisioning drake on the cloud machine, then you can leave this window open as you [read the textbook](http://manipulation.csail.mit.edu/intro.html).**\n",
        "\n",
        "# Notebook Setup\n",
        "\n",
        "The following cell will:\n",
        "- on Colab (only), install Drake to `/opt/drake`, install Drake's prerequisites via `apt`, and add pydrake to `sys.path`.  This will take approximately two minutes on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  If you navigate between notebooks using Colab's \"File->Open\" menu, then you can avoid provisioning a separate machine for each notebook.\n",
        "- define some utility methods/classes that will eventually disappear from this notebook and live in drake.\n",
        "\n",
        "You will need to rerun this cell if you restart the kernel, but it should be fast because the machine will already have drake installed."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab_type": "code",
        "id": "A4QOaw_zYLfI",
        "colab": {}
      },
      "source": [
        "import importlib\n",
        "import sys\n",
        "from urllib.request import urlretrieve\n",
        "\n",
        "# Install drake.\n",
        "if 'google.colab' in sys.modules and importlib.util.find_spec('pydrake') is None:\n",
        "  version='20200918'\n",
        "  build='nightly'\n",
        "  urlretrieve(f\"https://drake-packages.csail.mit.edu/drake/{build}/drake-{version}/setup_drake_colab.py\", \"setup_drake_colab.py\")\n",
        "  from setup_drake_colab import setup_drake\n",
        "  setup_drake(version=version, build=build)\n",
        "  !pip install pyngrok==4.2.2\n",
        "\n",
        "server_args = []\n",
        "if 'google.colab' in sys.modules:\n",
        "  server_args = ['--ngrok_http_tunnel']\n",
        "\n",
        "# Determine if this notebook is currently running as a notebook or a unit test.\n",
        "from IPython import get_ipython\n",
        "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
        "\n",
        "# TODO(russt): Consider teaching meshcat how to open the window on colab.\n",
        "def open_window(url, name):\n",
        "    if 'google.colab' in sys.modules:\n",
        "        # This has the benefit of not persisting between notebooks if the output \n",
        "        # is accidentally saved.\n",
        "        from google.colab import output\n",
        "        output.eval_js(f'window.open(\"{url}\", name);', ignore_result=True)\n",
        "    else:\n",
        "        from IPython.display import display, Javascript\n",
        "        display(Javascript(f'window.open(\"{url}\", name);'))\n",
        "\n",
        "# Imports\n",
        "import numpy as np\n",
        "from ipywidgets import ToggleButton, ToggleButtons\n",
        "\n",
        "import pydrake.all\n",
        "\n",
        "from pydrake.all import RigidTransform, RotationMatrix\n",
        "from pydrake.systems.jupyter_widgets import PoseSliders, WidgetSystem\n",
        "from pydrake.examples.manipulation_station import ManipulationStation\n",
        "\n",
        "pydrake.common.set_log_level(\"warn\")"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "C1sdq2R88C16"
      },
      "source": [
        "# Teleop Example (3D)\n",
        "\n",
        "In this example, we assemble a diagram with all of the relevant subsystems (the manipulation station, the meshcat visualizer, and some systems that provide a minimal teleop interface and convert the teleop output from end-effector commands into joint commands.  We'll learn more about each of these components in the following chapters.\n",
        "\n",
        "**NOTE:** If you command the robot to move its gripper beyond what is possible, then you get a message about \"differential IK\" failing.  I've left that in for now (rather than setting very conservative slider limits) partly because it has tutorial value.  We'll understand it more precisely soon!  For now, just stop the simulation and rerun the cell if you get stuck.\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab_type": "code",
        "id": "8wj7ZlyEw_AB",
        "colab": {}
      },
      "source": [
        "builder = pydrake.systems.framework.DiagramBuilder()\n",
        "\n",
        "station = builder.AddSystem(ManipulationStation())\n",
        "\n",
        "station.SetupClutterClearingStation()\n",
        "#ycb_objects = CreateClutterClearingYcbObjectList()\n",
        "#for model_file, X_WObject in ycb_objects:\n",
        "#    station.AddManipulandFromFile(model_file, X_WObject)\n",
        "station.AddManipulandFromFile(\n",
        "    \"drake/examples/manipulation_station/models/\"\n",
        "    + \"061_foam_brick.sdf\",\n",
        "    RigidTransform(RotationMatrix.Identity(), [0, -0.6, 0.2]))\n",
        "station.Finalize()\n",
        "\n",
        "visualizer = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(\n",
        "    builder, \n",
        "    station.get_scene_graph(), \n",
        "    station.GetOutputPort(\"pose_bundle\"),\n",
        "    zmq_url=\"new\",\n",
        "    open_browser=False,\n",
        "    server_args=server_args)\n",
        "#    jupyter_comms=True)\n",
        "\n",
        "robot = station.get_controller_plant()\n",
        "params = pydrake.manipulation.planner.DifferentialInverseKinematicsParameters(\n",
        "    robot.num_positions(), robot.num_velocities())\n",
        "\n",
        "time_step = 0.005\n",
        "params.set_timestep(time_step)\n",
        "# True velocity limits for the IIWA14 (in rad, rounded down to the first\n",
        "# decimal)\n",
        "iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])\n",
        "params.set_joint_velocity_limits((-iiwa14_velocity_limits,\n",
        "                                  iiwa14_velocity_limits))\n",
        "differential_ik = builder.AddSystem(\n",
        "    pydrake.manipulation.planner.DifferentialInverseKinematicsIntegrator(\n",
        "        robot, robot.GetFrameByName(\"iiwa_link_7\"), time_step, params))\n",
        "builder.Connect(differential_ik.get_output_port(),\n",
        "                station.GetInputPort(\"iiwa_position\"))\n",
        "\n",
        "teleop = builder.AddSystem(PoseSliders(\n",
        "    min_range = PoseSliders.MinRange(roll=0, pitch=-0.5, yaw=-np.pi, \n",
        "                                     x=-0.6, y=-0.8, z=0.0),\n",
        "    max_range = PoseSliders.MaxRange(roll=2*np.pi, pitch=np.pi, yaw=np.pi,\n",
        "                                     x=0.8, y=0.3, z=1.1)\n",
        "))\n",
        "builder.Connect(teleop.get_output_port(0), \n",
        "                differential_ik.get_input_port())\n",
        "wsg_buttons = ToggleButtons(value=0.107, description=\"SchunkWsg\", \n",
        "                            options=[('Open', 0.107), ('Close', 0.002)])\n",
        "wsg_teleop = builder.AddSystem(WidgetSystem([wsg_buttons]))\n",
        "builder.Connect(wsg_teleop.get_output_port(0),\n",
        "                station.GetInputPort(\"wsg_position\"))\n",
        "\n",
        "diagram = builder.Build()\n",
        "simulator = pydrake.systems.analysis.Simulator(diagram)\n",
        "context = simulator.get_mutable_context()\n",
        "\n",
        "station_context = station.GetMyMutableContextFromRoot(context)\n",
        "\n",
        "q0 = station.GetOutputPort(\"iiwa_position_measured\").Eval(\n",
        "    station_context)\n",
        "differential_ik.get_mutable_parameters().set_nominal_joint_position(q0)\n",
        "diff_ik_context = differential_ik.GetMyMutableContextFromRoot(context)\n",
        "differential_ik.SetPositions(diff_ik_context, q0)\n",
        "teleop.SetPose(differential_ik.ForwardKinematics(diff_ik_context))\n",
        "\n",
        "if running_as_notebook:  # Then we're not just running as a test on CI.\n",
        "    simulator.set_target_realtime_rate(1.0)\n",
        "\n",
        "    # Open the meshcat visualizer window (check your pop-up blocker).\n",
        "    open_window(visualizer.vis.url(), \"meshcat\")\n",
        "\n",
        "    stop_button = ToggleButton(value=False, description='Stop Simulation')\n",
        "    display(stop_button)\n",
        "    while not stop_button.value:\n",
        "        simulator.AdvanceTo(simulator.get_context().get_time() + 2.0)\n",
        "    stop_button.value = False\n",
        "  \n",
        "else:\n",
        "    simulator.AdvanceTo(0.1)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "4cTkwpJU8tGX"
      },
      "source": [
        "# Teleop Example (2D)\n",
        "\n",
        "Many of the core concepts in this class can be studied in 2D instead of 3D.  And everything is simpler/cleaner there,  including teleoperation!"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab_type": "code",
        "id": "7dimkJhd9DjP",
        "colab": {}
      },
      "source": [
        "builder = pydrake.systems.framework.DiagramBuilder()\n",
        "\n",
        "station = builder.AddSystem(ManipulationStation())\n",
        "station.SetupPlanarIiwaStation()\n",
        "station.AddManipulandFromFile(\n",
        "    \"drake/examples/manipulation_station/models/\"\n",
        "    + \"061_foam_brick.sdf\",\n",
        "    RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))\n",
        "# TODO(russt): Add planar joint to brick\n",
        "station.Finalize()\n",
        "\n",
        "visualizer = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(\n",
        "    builder, \n",
        "    station.get_scene_graph(), \n",
        "    station.GetOutputPort(\"pose_bundle\"), \n",
        "    zmq_url=\"new\",\n",
        "    open_browser=False,\n",
        "    server_args=server_args)\n",
        "#    jupyter_comms=True)\n",
        "visualizer.set_planar_viewpoint()\n",
        "\n",
        "robot = station.get_controller_plant()\n",
        "params = pydrake.manipulation.planner.DifferentialInverseKinematicsParameters(\n",
        "    robot.num_positions(), robot.num_velocities())\n",
        "\n",
        "time_step = 0.005\n",
        "params.set_timestep(time_step)\n",
        "iiwa14_velocity_limits = np.array([1.4, 1.3, 2.3])\n",
        "params.set_joint_velocity_limits((-iiwa14_velocity_limits,\n",
        "                                  iiwa14_velocity_limits))\n",
        "# These constants are in body frame.\n",
        "params.set_end_effector_velocity_gain([1, 0, 0, 0, 1, 1])\n",
        "differential_ik = builder.AddSystem(\n",
        "    pydrake.manipulation.planner.DifferentialInverseKinematicsIntegrator(\n",
        "        robot, robot.GetFrameByName(\"iiwa_link_7\"), time_step, params))\n",
        "builder.Connect(differential_ik.get_output_port(),\n",
        "                station.GetInputPort(\"iiwa_position\"))\n",
        "\n",
        "teleop = builder.AddSystem(PoseSliders(\n",
        "    min_range = PoseSliders.MinRange(roll=0, x=-0.6, z=0.0),\n",
        "    max_range = PoseSliders.MaxRange(roll=3.4, x=0.8, z=1.1),\n",
        "    visible = PoseSliders.Visible(pitch=False, yaw=False, y=False)\n",
        "))\n",
        "builder.Connect(teleop.get_output_port(0), \n",
        "                differential_ik.get_input_port())\n",
        "wsg_buttons = ToggleButtons(value=0.107, description=\"SchunkWsg\", \n",
        "                            options=[('Open', 0.107), ('Close', 0.002)])\n",
        "wsg_teleop = builder.AddSystem(WidgetSystem([wsg_buttons]))\n",
        "builder.Connect(wsg_teleop.get_output_port(0),\n",
        "                station.GetInputPort(\"wsg_position\"))\n",
        "\n",
        "diagram = builder.Build()\n",
        "simulator = pydrake.systems.analysis.Simulator(diagram)\n",
        "context = simulator.get_mutable_context()\n",
        "station_context = station.GetMyMutableContextFromRoot(context)\n",
        "\n",
        "q0 = station.GetOutputPort(\"iiwa_position_measured\").Eval(\n",
        "    station_context)\n",
        "differential_ik.get_mutable_parameters().set_nominal_joint_position(q0)\n",
        "diff_ik_context = differential_ik.GetMyMutableContextFromRoot(context)\n",
        "differential_ik.SetPositions(diff_ik_context, q0)\n",
        "teleop.SetPose(differential_ik.ForwardKinematics(diff_ik_context))\n",
        "\n",
        "if running_as_notebook:  # Then we're not just running as a test on CI.\n",
        "    simulator.set_target_realtime_rate(1.0)\n",
        "\n",
        "    # Open the meshcat visualizer window (check your pop-up blocker).\n",
        "    open_window(visualizer.vis.url(), \"meshcat\")\n",
        "\n",
        "    stop_button = ToggleButton(value=False, description='Stop Simulation')\n",
        "    display(stop_button)\n",
        "    while not stop_button.value:\n",
        "        simulator.AdvanceTo(simulator.get_context().get_time() + 2.0)\n",
        "    stop_button.value = False\n",
        "\n",
        "else:\n",
        "    simulator.AdvanceTo(0.1)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab_type": "code",
        "id": "_JIDhLfN3BSe",
        "colab": {}
      },
      "source": [
        ""
      ],
      "execution_count": null,
      "outputs": []
    }
  ]
}